//
// FILE	
// 				fum_kernell.c
// AUTHOR
//				Filipe Teixeira Campos
// PROPERTY
//				This code is property of Filipe Teixeira Campos.
//				This code has no warranty, use at Your own risk.
//				The intelectual property is of of Filipe Teixeira Campos.
//
// DESC		 
//				this is the code of the kernell
//

#include <fum_kernell.h>


//
// FUNCTION	
// 				interrupt for timer 0
// AUTHOR
//				Filipe Teixeira Campos
// DESC		 
//				. this function hadles the interrupt for timer 0
//				. it has the context switch
//				. it has the algorithm that calcultes next task to run
//				. algorithm dependes of : 
//									- sleep mode oh the task
//									- task is locked by another task (lock)
//
FUMFUNC SIGNAL (SIG_OVERFLOW0)
{

	register unsigned char aux;// asm("r0");
	register unsigned char i;//	asm("r1");
	register unsigned char auxTask;// asm("r2");
	
	//
	// SAVE CONTEXT OF OLD TASK
	// push registers
	asm("push R0;");asm("push R1;");asm("push R2;");
	asm("push R3;");asm("push R4;");asm("push R5;");
	asm("push R6;");asm("push R7;");asm("push R8;");
	asm("push R9;");asm("push R10;");asm("push R11;");
	asm("push R12;");asm("push R13;");asm("push R14;");
	asm("push R15;");asm("push R16;");asm("push R17;");
	asm("push R18;");asm("push R19;");asm("push R20;");
	asm("push R21;");asm("push R22;");asm("push R23;");
	asm("push R24;");asm("push R25;");asm("push R26;");
	asm("push R27;");asm("push R28;");asm("push R29;");
	asm("push R30;");asm("push R31;");
	// push SREG
	asm("in R0,0x3f;"); 		// has to keep SREG becasue maths messes with SREG
	asm("push R0;");			// push r0 that has SREG
	
	// clear SREG to make calculus here
	asm("clc;");asm("cln;");asm("clz;");asm("cls;");asm("clv;");asm("clt;");asm("clh;");
	
	// if there is no taskt, save stack of loop who is calling
	// or if there is tasks, but is none current task, so
	// is the main sleep loop !!
	if( fumega.numTasks==0 || fumega.curTask>fumega.numTasks )
	{
		// first call so is the main loop stack
		// save stack pointer
		fumega.ucSPL=inp(SPL);
		fumega.ucSPH=inp(SPH);					
	}
	
	//
	// if theres is tasks, make the schedule processing
	//
	if( fumega.numTasks > 0 )
	{
		// save context, if already make a task start
		if( fumega.curTask < fumega.numTasks )
		{
			// save stack pointer
			fumega.Task[fumega.curTask].ucSPL=inp(SPL);
			fumega.Task[fumega.curTask].ucSPH=inp(SPH);	

			// only if is running can pass to stop
			i=fumega.curTask;
			aux = fumega.Task[i].ucState;
			if( aux == FUMTSK_RUN ) 
			{
				aux = FUMTSK_STOP ;
				fumega.Task[i].ucState = aux ;
			}
		}

	
		// see if has tasks in sleep and treat them
		for( i=0;i<fumega.numTasks;i++)
		{
			if( fumega.Task[i].ucState == FUMTSK_SLEEP || 
				fumega.Task[i].ucState == FUMTSK_WAIT )
			{
				if(	fumega.Task[i].ucSleep!=0 ) 
					fumega.Task[i].ucSleep--;
					
				if( fumega.Task[i].ucSleep==0 && fumega.Task[i].ucState == FUMTSK_SLEEP )
				{
					aux=FUMTSK_STOP;
					fumega.Task[i].ucState = aux;
				}
			}
		}
		
		// sheduller
		// choose next task
		auxTask = fumega.curTask ;
		for( ;; )
		{
			fumega.curTask++;
			if( fumega.curTask < fumega.numTasks ){
				if( fumega.Task[fumega.curTask].ucState == FUMTSK_STOP )
					break;
			}						
			if( auxTask == fumega.curTask )
			{
				fumega.curTask=255;
				break;
			}
		}
		// end sheduller
			
		if( fumega.curTask<fumega.numTasks )
		{
			// get stack pointer
			outp(fumega.Task[fumega.curTask].ucSPL,SPL);
			outp(fumega.Task[fumega.curTask].ucSPH,SPH);	
			aux=FUMTSK_RUN;
			fumega.Task[fumega.curTask].ucState = aux;
			
			fumega.CpuRun++;
			
#ifdef FU_PROT_MODE
			aux = fumega.curTask;
			if( aux & 0x1 ) sbi( PORTC , 0x2 );
			else 			cbi( PORTC , 0x2 );
			if( aux & 0x2 ) sbi( PORTC , 0x4 );
			else			cbi( PORTC , 0x4 );
			if( aux & 0x4 ) sbi( PORTC , 0x8 );
			else			cbi( PORTC , 0x8 );	
#endif				
		}
		else
		{
			// tricky tem q ir para o sleep do main loop
			// get stack pointer
			outp(fumega.ucSPL,SPL);
			outp(fumega.ucSPH,SPH);	
			
			fumega.CpuFree++;
			
#ifdef FU_PROT_MODE
			sbi( PORTC , 0x2 );
			sbi( PORTC , 0x4 );
			sbi( PORTC , 0x8 );
#endif			
		}
	}
	else
	{
		fumega.CpuFree++;
	}
	
	// just for stats
	if( fumega.CpuFree==100 || fumega.CpuRun==100 )
	{
		fumega.CpuFree /= 2;
		fumega.CpuRun /= 2;
	}

	// GET NEW CONTEXT OF NEW TASK	
	// pop sREG
	asm("pop R0;");	
	asm("out 0x3f,R0;"); 	// pop SREG
	// pop registers
	asm("pop R31;");asm("pop R30;");
	asm("pop R29;");asm("pop R28;");asm("pop R27;");
	asm("pop R26;");asm("pop R25;");asm("pop R24;");
	asm("pop R23;");asm("pop R22;");asm("pop R21;");
	asm("pop R20;");asm("pop R19;");asm("pop R18;");
	asm("pop R17;");asm("pop R16;");asm("pop R15;");
	asm("pop R14;");asm("pop R13;");asm("pop R12;");
	asm("pop R11;");asm("pop R10;");asm("pop R9;");
	asm("pop R8;");asm("pop R7;");asm("pop R6;");
	asm("pop R5;");asm("pop R4;");asm("pop R3;");
	asm("pop R2;");asm("pop R1;");asm("pop R0;");
	
	// reload timer
	TCNT0 = FUM_T0_1MS ;			// 1 ms timer

	// reti make the pop of stack and put it in PC,great
	asm("reti;");
}



//
// FUNCTION	
// 				inits the kernel struct memory
// AUTHOR
//				Filipe Teixeira Campos
// DESC		 
//				. just initialize memory to expected values
//
FUMFUNC void FG_initKernell(void)
{
#ifdef FU_PROT_MODE
			sbi( PORTC , 0x2 );
			sbi( PORTC , 0x4 );
			sbi( PORTC , 0x8 );
#endif	

	fumega.numTasks=0;
	fumega.curTask=255;
	fumega.CpuFree = 0;
	fumega.CpuRun = 0;	
	asm("ret;");
}


//
// FUNCTION	
// 				add a task to the kernel
// AUTHOR
//				Filipe Teixeira Campos
// DESC		 
//				. this function put a task in the tasks array
//				. keep task properties
//
FUMFUNC void FG_addTask(void (*task)(),unsigned char prio)
{
	unsigned int iAux;
	
	// if max tasks, return
	if( fumega.numTasks == FUM_MAX_TASKS ) 
	{
		asm("ret;");
	}	
		
	fumega.Task[fumega.numTasks].ucTid = fumega.numTasks+1 ;
	fumega.Task[fumega.numTasks].ucPrio = prio ;
	fumega.Task[fumega.numTasks].ucState = FUMTSK_STOP;

	// address of function
	iAux = (unsigned int)task ;
	fumega.Task[fumega.numTasks].ucaStack[FUM_TASK_STACKSIZE-1] = iAux;
	fumega.Task[fumega.numTasks].ucaStack[FUM_TASK_STACKSIZE-2] = iAux>>8;
	
	
	// start of stack
	iAux = (unsigned int) &fumega.Task[fumega.numTasks].ucaStack[FUM_TASK_STACKSIZE-1] ;
	iAux -= FUM_NUM_REGS;
	fumega.Task[fumega.numTasks].ucSPL = iAux ;
	fumega.Task[fumega.numTasks].ucSPH = iAux>>8 ;
		
	// init stack
	// SREG of stack is init with global int enable 
	fumega.Task[fumega.numTasks].ucaStack[FUM_TASK_STACKSIZE-FUM_NUM_REGS] |= 0x80 ; 
	
	// inc number of tasks	
	fumega.numTasks ++;
	
	asm("ret;");
	
}




//
// FUNCTION	
// 				starts the kernel to run
// AUTHOR
//				Filipe Teixeira Campos
// DESC		 
//				. put the stack point at the dummy task stack point (slee mode loop)
//				. enable the timer 0 switch context on
//
FUMFUNC void FG_startKernell(void)
{
	unsigned int iAux;
	
	// stack for kernel shedule
	iAux = (unsigned int) &fumega.ucaStack[FUM_KRNL_STACKSIZE-1] ;
	fumega.ucSPL = iAux ;
	fumega.ucSPH = iAux>>8 ;
	
	// enable timer interrupt
	TCCR0 = 0x05 ;					// CK / 1024
	TCNT0 = FUM_T0_1MS ;			// 1 ms timer
	TIMSK |= 0x2 ;					// enable timer 1

	// put stck at kernell stack
	outp(fumega.ucSPL,SPL);
	outp(fumega.ucSPH,SPH);
	


	// put in sleep, ISR will awake
	set_sleep_mode (0);
	
	sei();							// enable ints
	
SLEEP_LOOP:
		sleep_mode ();
		goto SLEEP_LOOP;
		goto SLEEP_LOOP;
	
}


//
// FUNCTION	
// 				puts a task in sleep mode, can be used to do context timer swap
// AUTHOR
//				Filipe Teixeira Campos
// DESC		 
//				. put task in sleep mode
//				. keep the timer value and wait for switch time
//
void FG_tasksleep(unsigned char tid,unsigned short ms)
{
	register unsigned char i ;
	register unsigned char aux ;
	
	cli();							// disable int
	
	for(i=0;i<fumega.numTasks;i++)
	{
		if( fumega.Task[i].ucTid == tid )
		{
			aux=FUMTSK_SLEEP;
			fumega.Task[i].ucState = aux ;
			fumega.Task[i].ucSleep =  ms+1 ;
			break;
		}
	}
	
	sei();							// enable int

	// if not find, ret
	if( i!=fumega.numTasks ) 
	{
		// really sleep
		while( fumega.Task[i].ucState == FUMTSK_SLEEP ) {
			asm ("nop;");
		}
	}
		
}



//
// FUNCTION	
// 				locks a critical section/lock
// AUTHOR
//				Filipe Teixeira Campos
// DESC		 
//				. passing a tid and a lock id, the lock is lcoked by the task, 
//					and only this task can go on
//				. if lock is already locked by other task, task go to state WAIT
//
void FG_Lock(unsigned char tid,unsigned char lock)
{
	register unsigned char i,j ;
	register unsigned char aux ;

	if( lock>NUM_LOCKS-1)
		return ;
	
	cli();							// disable int

	// if lock is free, or is in use by the same task id
	if( fumega.aLocks[lock].ucState==FUMLCK_FREE ||
		fumega.aLocks[lock].ucTidOwner==tid	) 
	{
		aux=FUMLCK_INUSE;
		fumega.aLocks[lock].ucState=aux;
		fumega.aLocks[lock].ucTidOwner=tid;
		fumega.aLocks[lock].ucLocksIdx=255;
		
		sei();
		return;
	}
	
	// if is in use by other task
	
	// add record cursor
	fumega.aLocks[lock].ucLocksIdx++;
	
	// get the tid and record it
	j=fumega.aLocks[lock].ucLocksIdx;
	if( j==NUM_LOCKS )
		fumega.aLocks[lock].ucLocksIdx--;	
		
	fumega.aLocks[lock].ucLocksTid[j] = tid;
	
	// put the task in WAIT STATE
	for(i=0;i<fumega.numTasks;i++)
	{
		if( fumega.Task[i].ucTid == tid )
		{
			aux=FUMTSK_WAIT;
			fumega.Task[i].ucState = aux ;
			break;
		}
	}
	
	sei();							// enable int

	// if not find, ret
	if( i!=fumega.numTasks ) 
	{
		// really sleep
		while( fumega.Task[i].ucState == FUMTSK_WAIT ) {
			asm ("nop;");
		}
	}	
	
}



//
// FUNCTION	
// 				unlocks a critical section/lock
// AUTHOR
//				Filipe Teixeira Campos
// DESC		 
//				. passing a tid and a lock id, it unlocks a clock, and awakes task, 
//					that are in WAIT mode
//
void FG_UnLock(unsigned char tid,unsigned char lock)
{
	register unsigned char i,j ;
	register unsigned char aux ;

	if( lock>NUM_LOCKS-1)
		return ;
	
	cli();							// disable int
	
	// if lock in use by the task
	if( fumega.aLocks[lock].ucState==FUMLCK_INUSE &&
		fumega.aLocks[lock].ucTidOwner==tid	) 
	{
		// put it free
		aux=FUMLCK_FREE;
		fumega.aLocks[lock].ucState=aux;
		fumega.aLocks[lock].ucTidOwner=0;
		
		// unlock all locked tasks
		do
		{
			j=fumega.aLocks[lock].ucLocksIdx ;
			if( j<NUM_LOCKS )
			{
				// find task id and put it in sleep mode
				for(i=0;i<fumega.numTasks;i++)
				{
					// find the task
					if( fumega.Task[i].ucTid == fumega.aLocks[lock].ucLocksTid[j] && 
						fumega.Task[i].ucState==FUMTSK_WAIT )
					{
						aux=FUMTSK_SLEEP;
						fumega.Task[i].ucState = aux ;
					}
				}	
			}	
			else	
				break;
			
			
			// see next task
			fumega.aLocks[lock].ucLocksIdx--;
		}
		while( fumega.aLocks[lock].ucLocksIdx );
		
	}
	
	sei();							// enable int
	
}
